#include "relocation.h"




int64 computeSSD(const Mat&a, const Mat&b, const float scale)
{
	CV_Assert(a.type() == b.type());
	CV_Assert(a.type() == CV_8U || a.type() == CV_8UC3);
	CV_Assert(a.size() == b.size());

	
	
	Mat lowa, lowb;
	Mat ga, gb;
	auto ss = a.size();
	ss = CvSize(ss.width*scale, ss.height*scale);
	resize(a, lowa, ss);
	resize(b, lowb, ss);

	if (a.channels() == 1)
	{
		lowa.convertTo(lowa, CV_16S);
		lowb.convertTo(lowb, CV_16S);
		auto m = mean(lowa);
		auto mm = mean(lowb);
		for (size_t i = 0; i < lowa.rows; i++)
		{
			short * p = lowa.ptr<short>(i);
			short * pp = lowb.ptr<short>(i);
			for (size_t j = 0; j < lowa.cols; j++)
			{
				p[j] -= m(0);
				pp[j] -= mm(0);
			}
		}


	}
	else
	{
		lowa.convertTo(lowa, CV_16SC3);
		lowb.convertTo(lowb, CV_16SC3);
		auto m = mean(lowa);
		auto mm = mean(lowb);
		for (size_t i = 0; i < lowa.rows; i++)
		{

			auto p = lowa.ptr<Vec3s>(i);
			auto pp = lowb.ptr<Vec3s>(i);
			for (size_t j = 0; j < lowa.cols; j++)
			{
				p[j][0] -= m(0);
				p[j][1] -= m(1);
				p[j][2] -= m(2);
				pp[j][0] -= mm(0);
				pp[j][1] -= mm(1);
				pp[j][2] -= mm(2);
			}
		}

	}

	cv::GaussianBlur(lowa, ga, cv::Size(5, 5), 2);
	cv::GaussianBlur(lowb, gb, cv::Size(5, 5), 2);
	short * pa = (short*)ga.data;
	short * pb = (short*)gb.data;
	int length = ga.rows*ga.cols*ga.channels();
	int64 ssd = 0;
	for (size_t i = 0; i < length; i++)
	{
		ssd += (pa[i] - pb[i])*(pa[i] - pb[i]);
	}
	return ssd;

}